#include "DftCalc.h"

DftCalc::DftCalc(int DftMsecsWindow, int SampleRate, int BytesPerSample, int NumChannels)
{
    this->DftMsecsWindow = DftMsecsWindow;
    this->SampleRate = SampleRate;
    this->BytesPerSample = BytesPerSample;
    this->NumChannels = NumChannels;

    this->DftWindow_NumTimeSamples = this->SampleRate * this->DftMsecsWindow / 1000;
    if (this->DftWindow_NumTimeSamples % 2 != 0)
        this->DftWindow_NumTimeSamples++;
    this->DftWindow_NumTimeSamplesBytes = this->DftWindow_NumTimeSamples * BytesPerSample * NumChannels;

    this->DftWindow_NumFreqSamples = this->DftWindow_NumTimeSamples / 2 + 1;
    this->DftWindow_HertzPerSample = this->SampleRate / 2 / this->DftWindow_NumFreqSamples;

    this->fftInData = (double*) calloc(sizeof(double), DftWindow_NumTimeSamples);
    this->fftOutData = (fftw_complex *) calloc(DftWindow_NumFreqSamples, sizeof(fftw_complex));
	this->fftPlan = fftw_plan_dft_r2c_1d(DftWindow_NumTimeSamples, fftInData, fftOutData, 0);

    this->dctInData = (double*) calloc(sizeof(double), 3*DftWindow_NumTimeSamples);
	this->dctOutData = (double *) calloc(sizeof(double), 3*DftWindow_NumFreqSamples);
	this->dctPlan = fftw_plan_r2r_1d(DftWindow_NumTimeSamples, dctInData, dctOutData, FFTW_REDFT10, 0);

    this->outWavFile = NULL;
    this->outWavFileWritten = 0;
    this->outWavFileWrittenCount = 0;
/*
    //this->MaxDFTModule = 0;
    this->FreqToSkip = 200; // hertz
    this->FreqToSkip_Samples = FreqToSkip / DftWindow_HertzPerSample;
    this->MaxDFTModule_SkipFreq = 0;
*/
}

DftCalc::~DftCalc()
{
    fftw_destroy_plan(fftPlan);
    free(this->fftInData);
    free(this->fftOutData);

    fftw_destroy_plan(dctPlan);
    free(this->dctInData);
    free(this->dctOutData);

    if (outWavFile != NULL)
    {
        delete outWavFile;
    }
}

void* DftCalc::calculate_DFT_Mod(void *data)
{
    //REMEMBER, we suppose data is long DftWindow_NumTimeSamplesBytes
    // PA_SAMPLE_S16LE -->  /**< Signed 16 Bit PCM, little endian (PC) */

    // TODO, we should delegate the filling of fftInData to a routine mathcing the data type...

    double madMaxResol = pow(2, BytesPerSample * 8 /* num bits for data*/ ) / 2 + 1; // it's signed, so /2+1, right ???

    short * pData = (short *) data;
    for (int i=0; i<DftWindow_NumTimeSamples; i++)
    {
        fftInData[i] = 0;

        for (int c=0; c<NumChannels; c++)
        {
            fftInData[i] += pData[i*NumChannels + c];
        }

        fftInData[i]  = fftInData[i] / NumChannels / madMaxResol;
    }

    fftw_execute(fftPlan);

    double *fftOutMod = (double*) malloc(sizeof(double) * DftWindow_NumFreqSamples);
    double maxMod = 0;
    for (int j=0; j<DftWindow_NumFreqSamples; j++)
    {
        fftOutMod[j] = sqrt(fftOutData[j][0]*fftOutData[j][0] + fftOutData[j][1]*fftOutData[j][1]);
        if (fftOutMod[j] > maxMod)
            maxMod = fftOutMod[j] ;
    }

    for (int j=0; j<DftWindow_NumFreqSamples; j++)
    {
        fftOutMod[j] = fftOutMod[j] / maxMod;
    }

    return fftOutMod;
}


void* DftCalc::calculate_DFT_Phase(void *data)
{
    //REMEMBER, we suppose data is long DftWindow_NumTimeSamplesBytes
    // PA_SAMPLE_S16LE -->  /**< Signed 16 Bit PCM, little endian (PC) */

    // TODO, we should delegate the filling of fftInData to a routine mathcing the data type...

    double madMaxResol = pow(2, BytesPerSample * 8 /* num bits for data*/ ) / 2 + 1; // it's signed, so /2+1, right ???

    short * pData = (short *) data;
    for (int i=0; i<DftWindow_NumTimeSamples; i++)
    {
        fftInData[i] = 0;

        for (int c=0; c<NumChannels; c++)
        {
            fftInData[i] += pData[i*NumChannels + c];
        }

        fftInData[i]  = fftInData[i] / NumChannels / madMaxResol;
    }

    fftw_execute(fftPlan);

    double *fftOutPhase = (double*) malloc(sizeof(double) * DftWindow_NumFreqSamples);
    for (int j=0; j<DftWindow_NumFreqSamples; j++)
    {
        fftOutPhase[j] = (atan2(fftOutData[j][0], fftOutData[j][1]) + M_PI)/2.0/M_PI;
    }

    return fftOutPhase;
}

void* DftCalc::calculate_DCT_Mod(void *data)
{
    //REMEMBER, we suppose data is long DftWindow_NumTimeSamplesBytes
    // PA_SAMPLE_S16LE -->  /**< Signed 16 Bit PCM, little endian (PC) */

    // TODO, we should delegate the filling of fftInData to a routine mathcing the data type...

    double madMaxResol = pow(2, BytesPerSample * 8 /* num bits for data*/ ) / 2 + 1; // it's signed, so /2+1, right ???

    short * pData = (short *) data;
    for (int i=0; i<DftWindow_NumTimeSamples; i++)
    {
        dctInData[i] = 0;

        for (int c=0; c<NumChannels; c++)
        {
            dctInData[i] += pData[i*NumChannels + c];
        }

        dctInData[i]  = dctInData[i] / NumChannels / madMaxResol;
    }

    fftw_execute(dctPlan);

    double *fftOutMod = (double*) malloc(sizeof(double) * DftWindow_NumFreqSamples);
    double maxMod = 0;
    for (int j=0; j<DftWindow_NumFreqSamples; j++)
    {
        fftOutMod[j] = fabs(dctOutData[j]);
        if (fftOutMod[j] > maxMod)
            maxMod = fftOutMod[j] ;
    }

    for (int j=0; j<DftWindow_NumFreqSamples; j++)
    {
        fftOutMod[j] = fftOutMod[j] / maxMod;
    }

    return fftOutMod;
}


void DftCalc::appendToWav()
{
    if (outWavFile == NULL)
    {
        const int format = SF_FORMAT_WAV | SF_FORMAT_PCM_16;
        const int channels = 1;
        const int sampleRate = this->SampleRate;
        char fn[1024];
        sprintf(fn, "~/temp/rec.%d.wav", outWavFileWrittenCount);
        outWavFile = new SndfileHandle(fn, SFM_WRITE, format, channels, sampleRate);
        outWavFileWritten = 0;
    }

    /* rescale factor so that the peak is at 2^30 --> apparently this is independent of the resolution (i.e. it's re-re-scaled internally */
    double madMaxResol = pow(2, 30);

    int *buf = (int*) malloc(sizeof(int) * DftWindow_NumTimeSamples);
    for (int i=0; i<DftWindow_NumTimeSamples; i++)
    {
        buf[i] = (int)(fftInData[i] * madMaxResol);
    }
    outWavFile->write(buf, DftWindow_NumTimeSamples);
    free(buf);

    outWavFileWritten++;
    if (outWavFileWritten > 50)
    {
        delete outWavFile;
        outWavFile = NULL;
        outWavFileWrittenCount++;
    }

}
